import pyrealsense2 as rs
import numpy as np
import threading

class Camera():
    def __init__(self, width=480, height=270, frame_rate=30):
        self.pipeline = rs.pipeline()
        self.width = width
        self.height = height
        self.frame_rate = frame_rate

        self.latest_image = None
        self.latest_depth = None
        self.running = False
        self.lock = threading.Lock()

    def Open(self):
        config = rs.config()
        config.enable_stream(rs.stream.color, self.width, self.height, rs.format.bgr8, self.frame_rate)
        config.enable_stream(rs.stream.depth, self.width, self.height, rs.format.z16, self.frame_rate)
        self.pipeline.start(config)
        self.align_to = rs.stream.color
        self.align = rs.align(self.align_to)
        
        self.thread = threading.Thread(target=self._update_frames)
        self.running = True
        self.thread.start()

    def Close(self):
        self.running = False
        self.pipeline.stop()

    def _update_frames(self):
        while self.running:
            frames = self.pipeline.wait_for_frames()
            frames = self.align.process(frames)

            color_frame = frames.get_color_frame()
            depth_frame = frames.get_depth_frame()

            if not color_frame or not depth_frame:
                continue

            color_image = np.asanyarray(color_frame.get_data())
            depth_image = np.asanyarray(depth_frame.get_data())

            with self.lock:
                self.latest_image = color_image
                self.latest_depth = depth_image

    def GetImage(self):
        with self.lock:
            return (True, self.latest_image.copy()) if self.latest_image is not None else (False, None)

    def GetDepthImage(self):
        with self.lock:
            return (True, self.latest_depth.copy()) if self.latest_depth is not None else (False, None)   

if __name__ == "__main__":
    import time
    import cv2
    camera = Camera(640,480,60)
    camera.Open()
    try:
        while True:
            start_time = time.time()
            _,image = camera.GetImage()
            _,depth_image = camera.GetDepthImage()
            print((time.time()-start_time))
            if image is not None:
                image = image[272:400,272:400]
                cv2.imshow("Camera Feed", image)
                if cv2.waitKey(1) & 0xFF == ord('q'):
                    break
            else:
                print("No image")
                time.sleep(1)
    finally:
        camera.Close()
        cv2.destroyAllWindows()